// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2010 Manuel Yguel <manuel.yguel@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#ifndef EIGEN_COMPANION_H
#define EIGEN_COMPANION_H

// This file requires the user to include
// * Eigen/Core
// * Eigen/src/PolynomialSolver.h

namespace Eigen {

namespace internal {

#ifndef EIGEN_PARSED_BY_DOXYGEN

template<int Size>
struct decrement_if_fixed_size
{
	enum
	{
		ret = (Size == Dynamic) ? Dynamic : Size - 1
	};
};

#endif

template<typename _Scalar, int _Deg>
class companion
{
  public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar, _Deg == Dynamic ? Dynamic : _Deg)

	enum
	{
		Deg = _Deg,
		Deg_1 = decrement_if_fixed_size<Deg>::ret
	};

	typedef _Scalar Scalar;
	typedef typename NumTraits<Scalar>::Real RealScalar;
	typedef Matrix<Scalar, Deg, 1> RightColumn;
	// typedef DiagonalMatrix< Scalar, Deg_1, Deg_1 > BottomLeftDiagonal;
	typedef Matrix<Scalar, Deg_1, 1> BottomLeftDiagonal;

	typedef Matrix<Scalar, Deg, Deg> DenseCompanionMatrixType;
	typedef Matrix<Scalar, _Deg, Deg_1> LeftBlock;
	typedef Matrix<Scalar, Deg_1, Deg_1> BottomLeftBlock;
	typedef Matrix<Scalar, 1, Deg_1> LeftBlockFirstRow;

	typedef DenseIndex Index;

  public:
	EIGEN_STRONG_INLINE const _Scalar operator()(Index row, Index col) const
	{
		if (m_bl_diag.rows() > col) {
			if (0 < row) {
				return m_bl_diag[col];
			} else {
				return 0;
			}
		} else {
			return m_monic[row];
		}
	}

  public:
	template<typename VectorType>
	void setPolynomial(const VectorType& poly)
	{
		const Index deg = poly.size() - 1;
		m_monic = -poly.head(deg) / poly[deg];
		m_bl_diag.setOnes(deg - 1);
	}

	template<typename VectorType>
	companion(const VectorType& poly)
	{
		setPolynomial(poly);
	}

  public:
	DenseCompanionMatrixType denseMatrix() const
	{
		const Index deg = m_monic.size();
		const Index deg_1 = deg - 1;
		DenseCompanionMatrixType companMat(deg, deg);
		companMat << (LeftBlock(deg, deg_1) << LeftBlockFirstRow::Zero(1, deg_1),
					  BottomLeftBlock::Identity(deg - 1, deg - 1) * m_bl_diag.asDiagonal())
						 .finished(),
			m_monic;
		return companMat;
	}

  protected:
	/** Helper function for the balancing algorithm.
	 * \returns true if the row and the column, having colNorm and rowNorm
	 * as norms, are balanced, false otherwise.
	 * colB and rowB are respectively the multipliers for
	 * the column and the row in order to balance them.
	 * */
	bool balanced(RealScalar colNorm, RealScalar rowNorm, bool& isBalanced, RealScalar& colB, RealScalar& rowB);

	/** Helper function for the balancing algorithm.
	 * \returns true if the row and the column, having colNorm and rowNorm
	 * as norms, are balanced, false otherwise.
	 * colB and rowB are respectively the multipliers for
	 * the column and the row in order to balance them.
	 * */
	bool balancedR(RealScalar colNorm, RealScalar rowNorm, bool& isBalanced, RealScalar& colB, RealScalar& rowB);

  public:
	/**
	 * Balancing algorithm from B. N. PARLETT and C. REINSCH (1969)
	 * "Balancing a matrix for calculation of eigenvalues and eigenvectors"
	 * adapted to the case of companion matrices.
	 * A matrix with non zero row and non zero column is balanced
	 * for a certain norm if the i-th row and the i-th column
	 * have same norm for all i.
	 */
	void balance();

  protected:
	RightColumn m_monic;
	BottomLeftDiagonal m_bl_diag;
};

template<typename _Scalar, int _Deg>
inline bool
companion<_Scalar, _Deg>::balanced(RealScalar colNorm,
								   RealScalar rowNorm,
								   bool& isBalanced,
								   RealScalar& colB,
								   RealScalar& rowB)
{
	if (RealScalar(0) == colNorm || RealScalar(0) == rowNorm || !(numext::isfinite)(colNorm) ||
		!(numext::isfinite)(rowNorm)) {
		return true;
	} else {
		// To find the balancing coefficients, if the radix is 2,
		// one finds \f$ \sigma \f$ such that
		//  \f$ 2^{2\sigma-1} < rowNorm / colNorm \le 2^{2\sigma+1} \f$
		//  then the balancing coefficient for the row is \f$ 1/2^{\sigma} \f$
		//  and the balancing coefficient for the column is \f$ 2^{\sigma} \f$
		const RealScalar radix = RealScalar(2);
		const RealScalar radix2 = RealScalar(4);

		rowB = rowNorm / radix;
		colB = RealScalar(1);
		const RealScalar s = colNorm + rowNorm;

		// Find sigma s.t. rowNorm / 2 <= 2^(2*sigma) * colNorm
		RealScalar scout = colNorm;
		while (scout < rowB) {
			colB *= radix;
			scout *= radix2;
		}

		// We now have an upper-bound for sigma, try to lower it.
		// Find sigma s.t. 2^(2*sigma) * colNorm / 2 < rowNorm
		scout = colNorm * (colB / radix) * colB; // Avoid overflow.
		while (scout >= rowNorm) {
			colB /= radix;
			scout /= radix2;
		}

		// This line is used to avoid insubstantial balancing.
		if ((rowNorm + radix * scout) < RealScalar(0.95) * s * colB) {
			isBalanced = false;
			rowB = RealScalar(1) / colB;
			return false;
		} else {
			return true;
		}
	}
}

template<typename _Scalar, int _Deg>
inline bool
companion<_Scalar, _Deg>::balancedR(RealScalar colNorm,
									RealScalar rowNorm,
									bool& isBalanced,
									RealScalar& colB,
									RealScalar& rowB)
{
	if (RealScalar(0) == colNorm || RealScalar(0) == rowNorm) {
		return true;
	} else {
		/**
		 * Set the norm of the column and the row to the geometric mean
		 * of the row and column norm
		 */
		const RealScalar q = colNorm / rowNorm;
		if (!isApprox(q, _Scalar(1))) {
			rowB = sqrt(colNorm / rowNorm);
			colB = RealScalar(1) / rowB;

			isBalanced = false;
			return false;
		} else {
			return true;
		}
	}
}

template<typename _Scalar, int _Deg>
void
companion<_Scalar, _Deg>::balance()
{
	using std::abs;
	EIGEN_STATIC_ASSERT(Deg == Dynamic || 1 < Deg, YOU_MADE_A_PROGRAMMING_MISTAKE);
	const Index deg = m_monic.size();
	const Index deg_1 = deg - 1;

	bool hasConverged = false;
	while (!hasConverged) {
		hasConverged = true;
		RealScalar colNorm, rowNorm;
		RealScalar colB, rowB;

		// First row, first column excluding the diagonal
		//==============================================
		colNorm = abs(m_bl_diag[0]);
		rowNorm = abs(m_monic[0]);

		// Compute balancing of the row and the column
		if (!balanced(colNorm, rowNorm, hasConverged, colB, rowB)) {
			m_bl_diag[0] *= colB;
			m_monic[0] *= rowB;
		}

		// Middle rows and columns excluding the diagonal
		//==============================================
		for (Index i = 1; i < deg_1; ++i) {
			// column norm, excluding the diagonal
			colNorm = abs(m_bl_diag[i]);

			// row norm, excluding the diagonal
			rowNorm = abs(m_bl_diag[i - 1]) + abs(m_monic[i]);

			// Compute balancing of the row and the column
			if (!balanced(colNorm, rowNorm, hasConverged, colB, rowB)) {
				m_bl_diag[i] *= colB;
				m_bl_diag[i - 1] *= rowB;
				m_monic[i] *= rowB;
			}
		}

		// Last row, last column excluding the diagonal
		//============================================
		const Index ebl = m_bl_diag.size() - 1;
		VectorBlock<RightColumn, Deg_1> headMonic(m_monic, 0, deg_1);
		colNorm = headMonic.array().abs().sum();
		rowNorm = abs(m_bl_diag[ebl]);

		// Compute balancing of the row and the column
		if (!balanced(colNorm, rowNorm, hasConverged, colB, rowB)) {
			headMonic *= colB;
			m_bl_diag[ebl] *= rowB;
		}
	}
}

} // end namespace internal

} // end namespace Eigen

#endif // EIGEN_COMPANION_H
